/**
* @file polyline.cpp
* August 1, 2008
* 
* @brief Implementation of the POLYLINE, a specialized curve object.
*
* This class derives the interface "Curve", implementing all the
* specific features of a POLYLINE geometric object.
* 
* @author Antonio Miranda (amiranda@tecgraf.puc-rio.br )
* @author Andre Pereira (brabo@tecgraf.puc-rio.br)
* 
* @see Curve
* @see CurveFactory
*/

// includes
//----------
#include "polyLine.h"
#include "../point3d.h"

#include <stdio.h>
#include <string.h>

#include <algorithm>


using namespace std;


//
/////////////////////////////////////////////////////////////////////
PolyLine::PolyLine()
{
  m_points.clear();
  m_type = POLYLINE;
}

//
/////////////////////////////////////////////////////////////////////
PolyLine::~PolyLine()
{
  m_points.clear();
}

//
/////////////////////////////////////////////////////////////////////
void 
PolyLine::getName( char *name )
{
  strcpy (name, "polyline");
}

//
/////////////////////////////////////////////////////////////////////
vector<Point3D>
PolyLine::getCurvePoints()
{
  return m_points;
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::getSubdvision( int _nd,
                         double _ratio,
                         vector<Point3D>& _subdp )
{
  _subdp.resize( _nd+1 );
  int np = (int) m_points.size(); // number of points in polyline

  // parametric positions of points
  vector<double> parPos;
  getParamPositions( m_points, parPos ); 
  parPos[0] = 0.0;
  parPos[np-1] = 1.0;

  // parametric subdivision
  vector<double> parSubd;
  getParamSubdivion( _nd, _ratio, parSubd );
  parSubd[0] = 0.0;
  parSubd[_nd] = 1.0;

  // begin point
  _subdp[0] = m_points[0];

  // intermediary points
  int i,j,k;
  for (i = j = k = 1; i < np && k <= _nd; ++k)
  {
    for ( ; j < np && parPos[j] < parSubd[k]; ++j)	// finds k interval that fits s[j]
     ;
    {
      // finds d[k] with liner interpolation
      double t = (parSubd[k] - parPos[j-1]) / (parPos[j] - parPos[j-1]);
      _subdp[k] = (m_points[j-1] + t * (m_points[j] - m_points[j-1]));
    }
    i = j;
  }

  // end point
  _subdp[_nd] = m_points[np-1];
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::getTangent( int nd,
                      double ratio,
                      vector<Point3D>& tang )
{
  int i,j,k;
  int np = (int) m_points.size(); // number of points in polyline

  tang.resize (nd+1);

  // tangent on each point of polyline
  vector <Point3D> pt;
  pt.resize(np);
  for (i = 0; i < np-1; ++i)
    pt[i] = m_points[i+1] - m_points[i];
  pt[np-1] = m_points[np-1] - m_points[np-2];

  // parametric positions of points
  vector <double> parPos;
  getParamPositions (m_points, parPos); 
  parPos[0] = 0.0;
  parPos[np-1] = 1.0;

  // parametric subdvision
  vector <double> parSubd;
  getParamSubdivion (nd, ratio, parSubd);
  parSubd[nd] = 1.0;

  // begin point
  tang[0] = m_points[0];

  // intermediary points
  for (i = j = k = 1; i < np && k <= nd; ++k)
  {
    for ( ; j < np && parPos[j] < parSubd[k]; ++j)	// finds k interval that fits s[j]
     ;
    {
      // finds d[k] with liner interpolation
      // double t = (parSubd[k] - parPos[j-1]) / (parPos[j] - parPos[j-1]);
      // tang[k] = (pt[j-1] + t * (pt[j] - pt[j-1]));
      tang[k] = pt[j-1];
    }
    i = j;
  }

  // end point
  tang[nd] = m_points[np-1];
}

//
/////////////////////////////////////////////////////////////////////
double
PolyLine::getLenght()
{
  int i;
  double l = 0.0;
  Point3D  dv;

  for (i = 1; i < (int) m_points.size(); ++i)
  {
    dv = m_points[i] - m_points[i-1];
    l += dv.len ( );
  }
  return l;
}

// WHY DO WE NEED TOLERANCE HERE????
/////////////////////////////////////////////////////////////////////
int
PolyLine::clstPoint( Point3D pt,
                     double& t,
                     Point3D& pt_on,
                     double tol )
{
  static vector<double> s;
  int    np = (int) m_points.size();
  double d;
  Point3D intersTmp, inters;

  // get parametric position of p
  getParamPositions( m_points, s );
  s[0] = 0.0;
  s[np-1] = 1.0;

  double dmin = -1.0;
  int       k = -1;

  // for all peaces of lines
  for( int i = 0; i < np - 1; i++ )
  {
    distancePoint2Segment( m_points[i], m_points[i+1], pt, intersTmp, d );

    if( d < dmin || dmin == -1.0) 
    { 
      inters = intersTmp;
      dmin = d; 
      k = i; 
    }
  }

  // this test checks if the point was found
  if( k == -1 )    
     return 0;

  // parametric values of line
  double s0 = s[k];
  double s1 = s[k+1];

  // points of line
  Point3D p0 = m_points[k];
  Point3D p1 = m_points[k+1];

  // lenght of line
  double p1p0len = p1.distance( p0 );

  if( p1p0len == 0 )
    return 0;

  double ti = inters.distance( p0 ) / p1p0len;
  t = s0 + ti * (s1 - s0);
  pt_on = inters;

  // outside 
  if( (ti < 0.) || (ti > 1.) ) 
    return 0;

  // outside 
  double dp = pt.distance2( pt_on );

  // outside 
  if( dp > tol ) 
    return 0;

  return 1;
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::get3dPoint( double t,
                      Point3D& pt,
                      Point3D& tg )
{
  int i , n = (int) m_points.size();
  double tlenght=0,lenght=0;

  // get current length
  for (i = 1; i < n; ++i)
    tlenght += m_points[i].distance(m_points[i-1]);

  // get direct point in t = 0
  if (t == 0.0)
  {
    pt = m_points[0];
    tg = m_points[1] - m_points[0];
    tg.normalize (tg);
    tg *= tlenght;
    return;
  }

  // get direct point in t = 1
  if (t == 1.0)
  {
    pt = m_points[n-1];
    tg = m_points[n-1] - m_points[n-2];
    tg.normalize (tg);
    tg *= tlenght;
  }


  for (i = 1; i < n; ++i)
  {
    double ti,tj;
    int   j = i-1;
    double dt = m_points[i].distance(m_points[j]);

    if (dt<0.000001) 
      continue;

    tj = lenght / tlenght;
    lenght += dt;
    ti = lenght / tlenght;

    if (i == n-1 || ti > t)
    {
      double tt = (t-tj) / (ti-tj);
      tg = m_points[i] - m_points[j];
      pt = m_points[j] + tt * tg;
      tg.normalize (tg);
      tg *= tlenght;
      return;
    }
  }
}


//
/////////////////////////////////////////////////////////////////////
Point3D
PolyLine::get3dPoint( double _t )
{
  Point3D pt;
  int i , n = (int) m_points.size();
  double tlenght=0,lenght=0;

  if (_t == 0.0)
    return m_points[0];
  if (_t == 1.0)
    return m_points[n-1];

  for (i = 1; i < n; ++i)
    tlenght += m_points[i].distance(m_points[i-1]);

  for (i = 1; i < n; ++i)
  {
    double ti,tj;
    int   j = i-1;
    double dt = m_points[i].distance(m_points[j]);

    if (dt<0.000001) 
      continue;

    tj = lenght / tlenght;
    lenght += dt;
    ti = lenght / tlenght;

    if(i == n-1 || ti > _t)
    {
      double tt = (_t-tj) / (ti-tj);
      pt = m_points[j] + tt * (m_points[i] - m_points[j]);
      break;
    }
  }
  return pt;
}


//
/////////////////////////////////////////////////////////////////////
void
PolyLine::draw()
{
  int i;

  if (m_pDrawObj == NULL)
    return;

  if( !this->possible() )
    return;

  m_pDrawObj->begin();

  for (i = 0; i < (int) m_points.size(); ++i)
    m_pDrawObj->addVertex(m_points[i].getX(), m_points[i].getY(), m_points[i].getZ());

  m_pDrawObj->end();
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::setBox()
{
  int i;
  m_box.clear();
  for (i = 0; i < (int)m_points.size(); ++i)
    m_box.update( m_points[i].getX(), m_points[i].getY(), m_points[i].getZ() ); 
}



//
/////////////////////////////////////////////////////////////////////
int
PolyLine::beginAddPoint( const vector<Point3D>& _addp )
{ 
  return 1;
}

//
/////////////////////////////////////////////////////////////////////
int
PolyLine::addPoint( Point3D& _pt )
{
  // It is allowed to coincide the points in Polylines because it may be closed:
  // exception just to the first and second inserted points:
  if( m_points.size() == 1 )
  {
    if( m_points[0].distance(_pt) == 0.0 )
      return 0;
  }
  m_points.push_back( _pt );
  m_firstDraw = true;
  return 1;
}

//
/////////////////////////////////////////////////////////////////////
int
PolyLine::addPoint( Point3D& _pt,
                    double tol )
{
  // It is allowed to coincide the points in Polylines because it may be closed:
  // exception just to the first and second inserted points:
  if( m_points.size() == 1 )
  {
    if( m_points[0].distance(_pt) < tol )
      return 0;
  }
  m_points.push_back(_pt);
  m_firstDraw = true;
  return 1;
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::addTempPoint( Point3D& pt )
{
  int j;
  int n = (int) m_points.size();

  if (m_pDrawObj == NULL)
    return;
  if (n < 1)
    return;

  for (j = 0; j < 2; j++)
  {
    m_pDrawObj->setXor(1);

    if (m_firstDraw)
    {
      draw(pt);
      m_lastPt = pt;
      m_firstDraw = false;
      m_pDrawObj->end();
      m_pDrawObj->setXor(0);
      break;
    }
    else
    {
      if (j == 0)
      {
        draw (m_lastPt);
      }
      else
      {
        draw (pt);
        m_lastPt = pt;
      }
    }
    m_pDrawObj->setXor(0);

  }
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::addTempOrthoPoint( Point3D& pt )
{
  int j;
  int n = (int) m_points.size();

  if (m_pDrawObj == NULL)
    return;
  if (n < 1)
    return;

  for (j = 0; j < 2; j++)
  {
    m_pDrawObj->setXor(1);

    if (m_firstDraw)
    {
      draw(pt);
      m_lastPt = pt;
      m_firstDraw = false;
      m_pDrawObj->end();
      m_pDrawObj->setXor(0);
      break;
    }
    else
    {
      if (j == 0)
      {
        draw (m_lastPt);
      }
      else
      {
        Point3D pt_diff = pt - m_points.back();

        double abs_x = abs( (int) pt_diff.getX());
        double abs_y = abs( (int) pt_diff.getY());
        double abs_z = abs( (int) pt_diff.getZ());

        if ( (abs_x > abs_y) && (abs_x > abs_z) )
        {
          pt_diff.setY(0);
          pt_diff.setZ(0);
        }
        else if ( abs_y > abs_z )
        {
          pt_diff.setX(0);
          pt_diff.setZ(0);
        }
        else
        {
          pt_diff.setX(0);
          pt_diff.setY(0);
        }
        pt = m_points.back() + pt_diff;

        draw (pt);
        m_lastPt = pt;
      }
    }
    m_pDrawObj->setXor(0);

  }
}

//
/////////////////////////////////////////////////////////////////////
Point3D
PolyLine::getLastTempPoint()
{ 
  return m_lastPt;
}

//
/////////////////////////////////////////////////////////////////////
int
PolyLine::endAddPoint()
{ 
  if( !possible() )
    return 0;
  setBox();
  return 1;
}

/////////////////////////////////////////////////////////////////////
void
PolyLine::getEndPoints( Point3D pt[2] )
{
  int n = (int)m_points.size();

  if( n == 0 )
	  return;

  pt[0] = m_points[0];
  pt[1] = m_points[n-1];
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::delPoint( Point3D& pt )
{
  pt = m_points[((int)m_points.size ()) - 1];
  m_points.pop_back ( ); // remove the last point
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::delPoint( Point3D& pt,
                    double tol )
{
  int  i;

  for (i = 0; i < (int) m_points.size(); ++i)
  {
    Point3D dv = m_points[i] - pt;
    if (dv.len() < tol)
    {
      m_points.erase (m_points.begin()+i);
      return;
    }
  }
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::copy( Curve** cp )
{
  *cp = new PolyLine;
  PolyLine *subobj = (PolyLine *) *cp;
  subobj->m_points = this->m_points;
  subobj->m_type = this->m_type;
  subobj->m_subdv = this->m_subdv;
  subobj->m_ratio = this->m_ratio;
  subobj->m_attr = this->m_attr;
  subobj->m_box = this->m_box;
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::transform( double m[4][4] )
{
  int i;
  for (i = 0; i < (int) m_points.size(); ++i)
  {
    Point3D a = m_points[i];
    double w = m[3][0]*a.getX()+m[3][1]*a.getY()+m[3][2]*a.getZ()+m[3][3];
    m_points[i].setX ((m[0][0]*a.getX() + m[0][1]*a.getY() + m[0][2]*a.getZ() + m[0][3]) / w);
    m_points[i].setY ((m[1][0]*a.getX() + m[1][1]*a.getY() + m[1][2]*a.getZ() + m[1][3]) / w);
    m_points[i].setZ ((m[2][0]*a.getX() + m[2][1]*a.getY() + m[2][2]*a.getZ() + m[2][3]) / w);
  }
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::invert()
{
  int i, n = (int) m_points.size();
  for (i = 0; i < n / 2; ++i)
  {
    Point3D tmp = m_points[i];
    m_points[i] = m_points[n-i-1];
    m_points[n-i-1] = tmp;
  }
}

//
/////////////////////////////////////////////////////////////////////
int
PolyLine::possible()
{
  if ((int)m_points.size() < 2)
    return 0;
  return 1;
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::print()
{
  cout << " - Polyline " << endl;
  int np = (int) m_points.size();
  for( int i = 0; i < np; ++i )
    cout << "   p"<<i<<": ( "<<m_points[i].getX()<<"; "<<m_points[i].getY()<<"; "<<m_points[i].getZ()<<" )" << endl;
}



//
/////////////////////////////////////////////////////////////////////
int
PolyLine::planeIntersect( Point3D& _n,
                         double _d,
                         vector<Curve*>& _curves,
                         vector<Point3D>& _pts)
{
  // compute intersection for each line segment, also using
  // the algorithm: O'Rourke - Computational Geometry in C
  // Code 7.7 - pg. 231

  _curves.clear();
  _n.normalize();
  double t;  // parametric value from 0 to 1
  int k = 0;  // index to identify the last segment to intersect
  int npts = (int) m_points.size();
  Point3D tempPt;

  for( int i=1; i<npts; i++ )
  {
    // analyzes each segment
    double num = -1.0*_d - m_points[i-1].dotProd( _n );
    double denom = _n.dotProd( m_points[i] - m_points[i-1] );

    if( denom != 0.0 ) // segment is not parallel to plane
    {
      t = num / denom;  // parametric value (0-1) where intersection occurs
      if( (0.0 < t) && (t < 1.0) )
      {
        // computes the intersecting point
        Point3D newPt( m_points[i-1] + t * ( m_points[i] - m_points[i-1]) );
        // creates a new curve
        Curve* polyLine_i = new PolyLine();
        // defines a POLYLINE with (i-k+1) points
        if( k != 0 )  // adds the first point
          polyLine_i->addPoint( tempPt );
        // adds the intermediate points
        for( int j=k; j<i; j++ )
          polyLine_i->addPoint( m_points[j] );
        // adds the final point
        polyLine_i->addPoint( newPt );

        // updates values
        tempPt = newPt;
        k = i;
        // obj to be returned with the new curves created due to the intersection
        _curves.push_back( polyLine_i );
        _pts.push_back (newPt);
      }
    }
  }
  if( k == 0 )
    return 0;
  else  // at least one intersection found
  {
    // creates the last curve
    Curve* polyLine = new PolyLine();
    // defines a POLYLINE with (i-k+1) points
    if( k != 0 )  // adds the first point
      polyLine->addPoint( tempPt );
    // adds the remaining points
    for( int j=k; j<npts; j++ )
      polyLine->addPoint( m_points[j] );
    // obj to be returned with the new curves created due to the intersection
    _curves.push_back( polyLine );
    return 1;
  }
}

//
/////////////////////////////////////////////////////////////////////
int
PolyLine::planeIntersect( Point3D& _orig,
                         Point3D& _n,
                         vector<Curve*>& _curves,
                         vector<Point3D>& _pts)
{
  // finds the minimal distance from origin to the plane
  _n.normalize();
  double d = _orig.dotProd( _n );
  return planeIntersect( _n, d, _curves,_pts);
}

//
/////////////////////////////////////////////////////////////////////
int
PolyLine::join( Curve* _givenCurve,
                Curve** _thirdCurve )
{
  //---------------------------------------------------
  // IMPLEMENTATIONs for the FUTURE: it should return a JOINED curve
  //---------------------------------------------------
  if( (_givenCurve->getType() == Curve::POLYLINE)  )
  {
    double tol = 1.0e-5;
    PolyLine* newCurve = new PolyLine;
    PolyLine* givenCurve = (PolyLine *)_givenCurve;
    // tests if an endpoint has the same coordinates of a start point of the other
    int i;
    int npts1 = (int)m_points.size();
    int npts2 = (int)givenCurve->m_points.size();
    if( m_points[0].isAtSamePosition( givenCurve->m_points[0], tol ) )
    {
      for(i = npts1-1; i > 0; --i)
        newCurve->addPoint(m_points[i]);
      for(i = 0; i < npts2; ++i)
        newCurve->addPoint(givenCurve->m_points[i]);
    }
    else if( m_points[0].isAtSamePosition( givenCurve->m_points[npts2-1], tol ) )
    {
      for(i = npts1-1; i > 0; --i)
        newCurve->addPoint(m_points[i]);
      for(i = npts2-1; i >= 0; --i)
        newCurve->addPoint(givenCurve->m_points[i]);
    }
    else if( m_points[npts1-1].isAtSamePosition( givenCurve->m_points[0], tol ) )
    {
      for(i = 0; i < npts1; ++i)
        newCurve->addPoint(m_points[i]);
      for(i = 1; i < npts2; ++i)
        newCurve->addPoint(givenCurve->m_points[i]);
    }
    else if( m_points[npts1-1].isAtSamePosition( givenCurve->m_points[npts2-1], tol ) )
    {
      for(i = 0; i < npts1; ++i)
        newCurve->addPoint(m_points[i]);
      for(i = npts2-2; i >= 0; --i)
        newCurve->addPoint(givenCurve->m_points[i]);
    }
    newCurve->endAddPoint();
    *_thirdCurve = newCurve;
    return 1;
  }
  return 0;
}

// TODO: checks return statement, ONLY SPLIT if CURVE has at least 2 points?
/////////////////////////////////////////////////////////////////////
int
PolyLine::split( vector<Curve*>& _curves )
{
  split( m_subdv, m_ratio, _curves );
  return 1;
}

// TODO: checks return statement, ONLY SPLIT if CURVE has at least 2 points?
/////////////////////////////////////////////////////////////////////
int
PolyLine::split( int _subdvision,
                 double _ratio,
                 vector<Curve*>& _curves )
{
  // gets the subdivision points for the curve and their parametric positions 
  vector<Point3D> subv;
  vector<double> parPos;
  getSubdvision( _subdvision, _ratio, subv );
  getParamPositions( subv, parPos ); 
  
  // gets the parametric values for the vertices of the POLYLINE
  vector<double> s;
  getParamPositions( m_points, s );
 
  // loop over the number of subdivision points to build the new curves
  int size = (int) subv.size();
  int k = 1;
  for( int i=1; i<size; i++ )
  {
    // creates a new curve (POLYLINE)
    Curve* curve = new PolyLine();
    // adds a pair of subdivision points to the curve
    curve->addPoint( subv[i-1] );
    while( (parPos[i] > s[k]) && (parPos[i] < 1.) )  // the second statement is strange!!!
    {
      curve->addPoint( m_points[k] );
      k++;
    }
    curve->addPoint( subv[i] );
    // adds the new curve to the list
    _curves.push_back( curve );
  }
  return 1;
}

// TODO: checks return statement, ONLY SPLIT if CURVE is completed?
/////////////////////////////////////////////////////////////////////
Curve* 
PolyLine::split( double _t0, double _t1, double _tol )
{
  Curve* polyline = NULL;

  // if the interval is from 0 to 1 then it creates a copy of the curve
  if( (_t0==0.0) && (_t1==1.0) )
  {
    this->copy( &polyline );
  }
  else
  {
    // creates a new curve (POLYLINE)
    polyline = new PolyLine;

    int np = (int)m_points.size();

    if( np > 0 )
    {
      // gets the parametric values for the vertices of the POLYLINE
      vector<double> s;
      getParamPositions( m_points, s );

      // adds a pair of subdivision points to the curve
      Point3D _pts = get3dPoint(_t0);
      polyline->addPoint( _pts );

      int npts = (int) s.size();
      for (int k = 0; k < npts; ++k)
      {
        if (s[k] > _t0 && s[k] < _t1)
          polyline->addPoint( m_points[k] );
      }
      
      _pts = get3dPoint(_t1);
      polyline->addPoint( _pts );
    }
  }

  return polyline;
}

//
/////////////////////////////////////////////////////////////////////
double
PolyLine::getSmallestSize()
{
  int i;
  double l = -1.0;
  Point3D  dv;

  for (i = 1; i < (int) m_points.size(); ++i)
  {
    dv = m_points[i] - m_points[i-1];
    double ll = dv.len ( );
    if (l < 0.0 || ll < l ) 
      l = ll;
  }
  return l;
}

//
/////////////////////////////////////////////////////////////////////
void
PolyLine::draw( Point3D& pt )
{
  int i;
  int n = (int) m_points.size();
  static char text[50];

  if (n < 1)
    return;

  // draw polyline
  m_pDrawObj->begin();
  {
    for (i = 0; i < n; ++i)
      m_pDrawObj->addVertex(m_points[i].getX(), m_points[i].getY(), m_points[i].getZ());

    m_pDrawObj->addVertex(pt.getX(), pt.getY(), pt.getZ());
  }
  m_pDrawObj->end();

  // get fix length
  if (n > 1)
  {
    double len = getLenght();
    sprintf(text, "L=%.3f", len);
    m_pDrawObj->text(m_points[n-1].getX(), m_points[n-1].getY(), m_points[n-1].getZ(), text);
  }

  // draw text update length
  sprintf(text, "l=%.3f", m_points[n-1].distance(pt));
  Point3D c = (m_points[n-1] + pt) / 2.0;
  m_pDrawObj->text(c.getX(), c.getY(), c.getZ(), text);
}



//
/////////////////////////////////////////////////////////////////////
void
PolyLine::updateData( int dg, int nP, int nU, double* P, double* U )
{
  // setting the read data in the curve variables:
  m_points.clear();
  for(int i = 0; i < nP; ++i)
  {
    Point3D pt( P[4*i+0], P[4*i+1], P[4*i+2] );
    m_points.push_back( pt );
  }
}
